#include <QtGui>
#include <QMessageBox>
#include <iostream>
#include "../include/kinova_driver_qt/main_window.hpp"

namespace kinova_driver_qt
{

using namespace Qt;


MainWindow::MainWindow(int argc, char** argv, QWidget *parent)
    : QMainWindow(parent)
    , qnode(argc, argv, 3315)
{
    ui.setupUi(this);
    QObject::connect(ui.actionAbout_Qt, SIGNAL(triggered(bool)), qApp, SLOT(aboutQt()));


    setWindowIcon(QIcon(":/images/icon.png"));
    ui.tab_manager->setCurrentIndex(0);

    ui.view_logging->setModel(qnode.loggingModel());

    QObject::connect( &qnode, SIGNAL(loggingUpdated()), this, SLOT(updateLoggingView() ) );
    QObject::connect( &qnode, SIGNAL(rosShutdown()),    this, SLOT(close() ) );

    QObject::connect( &qnode, SIGNAL( kinova_isReady() ), this, SLOT( enableGui() ) );

    //QObject::connect( ui.statusAutoRead_checkBox, SIGNAL(toggled(bool)),
    //                  this, SLOT(on_statusAutoRead_changed(bool)));

    QObject::connect( ui.reachPos_pushButton,  SIGNAL(clicked()), this, SLOT( on_reachPosBtn_clicked() ) );
    QObject::connect( ui.setJoints_pushButton, SIGNAL(clicked()), this, SLOT( on_reachAngularPosBtn_clicked() ) );
    QObject::connect( ui.setFingers_pushButton,SIGNAL(clicked()), this, SLOT( on_setFingersBtn_clicked() ) );

    QObject::connect(ui.emulateHome_pushButton, SIGNAL(clicked()),
                     &qnode, SLOT( emulate_homePos() ) );

    QObject::connect( ui.home_pushButton, SIGNAL(clicked()), &qnode, SLOT( gotoHomePos() ) );

    //QObject::connect( ui.addTrajectory_pushButton, SIGNAL(clicked()), this, SLOT( add_trajectory() ) );

    QObject::connect( ui.getStatus_pushButton, SIGNAL(clicked()), this, SLOT(on_getStatusBtn_clicked()));
    QObject::connect( ui.ping_pushButton, SIGNAL(clicked()), &qnode, SLOT(ping_kinova()) );
    QObject::connect(ui.navigation_pushButton, SIGNAL(clicked()),&qnode,SLOT(gotoNavigationPos()));

    statusAutoRead = true;

    qnode.init();
    ui.statusbar->showMessage("Trying to connect to Kinova_core_driver");
}

MainWindow::~MainWindow()
{
}

void MainWindow::showNoMasterMessage()
{
    QMessageBox msgBox;
    msgBox.setText("Couldn't find the ros master.");
    msgBox.exec();
    close();
}

void MainWindow::updateLoggingView()
{
    ui.view_logging->scrollToBottom();
}
void MainWindow::on_actionAbout_triggered()
{
    //QMessageBox::about(this, tr("About ..."),tr("<h2>PACKAGE_NAME Test Program 0.10</h2><p>Copyright Yujin Robot</p><p>This package needs an about description.</p>"));
}

void MainWindow::closeEvent(QCloseEvent *event)
{
    QMainWindow::closeEvent(event);
}

void MainWindow::on_kinovaStatus_updated(Status kinovaStatus)
{
    if(!statusAutoRead)
        return;

    ui.xpos_spinBox->setValue(kinovaStatus.cartesian.position.x);
    ui.ypos_spinBox->setValue(kinovaStatus.cartesian.position.y);
    ui.zpos_spinBox->setValue(kinovaStatus.cartesian.position.z);

    ui.xrot_spinBox->setValue(kinovaStatus.cartesian.rotation.x);
    ui.yrot_spinBox->setValue(kinovaStatus.cartesian.rotation.y);
    ui.zrot_spinBox->setValue(kinovaStatus.cartesian.rotation.z);

    ui.joint1_spinBox->setValue(kinovaStatus.joints.joint1);
    ui.joint2_spinBox->setValue(kinovaStatus.joints.joint2);
    ui.joint3_spinBox->setValue(kinovaStatus.joints.joint3);
    ui.joint4_spinBox->setValue(kinovaStatus.joints.joint4);
    ui.joint5_spinBox->setValue(kinovaStatus.joints.joint5);
    ui.joint6_spinBox->setValue(kinovaStatus.joints.joint6);

    ui.finger1_spinBox->setValue(kinovaStatus.fingers.finger1);
    ui.finger2_spinBox->setValue(kinovaStatus.fingers.finger2);
    ui.finger3_spinBox->setValue(kinovaStatus.fingers.finger3);
}

void MainWindow::on_statusAutoRead_changed(bool checked)
{
    statusAutoRead = checked;
}

void MainWindow::on_reachPosBtn_clicked()
{
    Cartesian cartesian;

    cartesian.position.x = ui.xpos_spinBox->value();
    cartesian.position.y = ui.ypos_spinBox->value();
    cartesian.position.z = ui.zpos_spinBox->value();

    cartesian.rotation.x = ui.xrot_spinBox->value();
    cartesian.rotation.y = ui.yrot_spinBox->value();
    cartesian.rotation.z = ui.zrot_spinBox->value();

    qnode.reach_cartesianPos(cartesian);
}

void MainWindow::add_trajectory()
{
    Cartesian trajectory;

    trajectory.position.x = ui.xpos_spinBox->value();
    trajectory.position.y = ui.ypos_spinBox->value();
    trajectory.position.z = ui.zpos_spinBox->value();

    trajectory.rotation.x = ui.xrot_spinBox->value();
    trajectory.rotation.y = ui.yrot_spinBox->value();
    trajectory.rotation.z = ui.zrot_spinBox->value();

    QString index = "trajectory_"+QString::number(savedTrajectories.size()+1);
    savedTrajectories[index] = trajectory;
    //ui.trajectories_listWidget->addItem(index);

}

void MainWindow::on_reachAngularPosBtn_clicked()
{
    Joints joints;
    joints.joint1 = ui.joint1_spinBox->value();
    joints.joint2 = ui.joint2_spinBox->value();
    joints.joint3 = ui.joint3_spinBox->value();
    joints.joint4 = ui.joint4_spinBox->value();
    joints.joint5 = ui.joint5_spinBox->value();
    joints.joint6 = ui.joint6_spinBox->value();
    qnode.reach_angularPos(joints);
}

void MainWindow::on_setFingersBtn_clicked()
{
    Fingers fingers;
    fingers.finger1 = ui.finger1_spinBox->value();
    fingers.finger2 = ui.finger2_spinBox->value();
    fingers.finger3 = ui.finger3_spinBox->value();
    qnode.set_fingers(fingers);
}

void MainWindow::enableGui(bool enable)
{
    ui.home_pushButton->setEnabled(enable);
    ui.setFingers_pushButton->setEnabled(enable);
    ui.setJoints_pushButton->setEnabled(enable);
    ui.reachPos_pushButton->setEnabled(enable);
    ui.navigation_pushButton->setEnabled(enable);
}

void MainWindow::on_getStatusBtn_clicked()
{
    Status status;
    if(qnode.getStatus(&status))
    {
        ui.statusbar->showMessage("Connection to kinova esatblished successfully",2000);
        ROS_INFO("Status Received Updating GUI");
        on_kinovaStatus_updated(status);
        enableGui(true);
    }

    else
    {
        ui.statusbar->showMessage("Connection to kinova failed",1000);
        ROS_ERROR("Getting Status failed");
        enableGui(false);
    }
}
}

